#include "CAN_Task.h"
#include "bsp_can.h"
#include "cmsis_os.h"
#include "Motor.h"
#include "pid.h"
#include "usart.h"
#include "remote_control.h"
#include "math.h"
#include "arm_math.h"
#include "Control_Task.h"
#include "INS_Task.h"
#include "bsp_uart.h"


static void Motor_ON();
static void Motor_OFF();


 void CAN_Task(void const * argument)
{
	MIT_Motor_Command(&FDCAN2_TxFrame,0x01,Motor_Enable);
		osDelay(100);
	MIT_Motor_Command(&FDCAN2_TxFrame,0x02,Motor_Enable);
	osDelay(100);
	MIT_Motor_Command(&FDCAN2_TxFrame,0x01,Motor_Enable);
	osDelay(100);
	MIT_Motor_Command(&FDCAN2_TxFrame,0x02,Motor_Enable);
	osDelay(100);
	MIT_Motor_Command(&FDCAN2_TxFrame,0x03,Motor_Enable);
	osDelay(100);
	MIT_Motor_Command(&FDCAN2_TxFrame,0x04,Motor_Enable);
	osDelay(100);

	for(;;)
  {
	if(Control_Info.Chassmis_Mode == CHASSMIS_WEAK)
		Motor_OFF();
else
	Motor_ON();

		
		
		osDelay(1);
  }
 
}

void Motor_OFF(){

 

  GIM_Motor_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&GIM_8108_Motor[0],0,0,0,0,0);
  GIM_Motor_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&GIM_8108_Motor[1],0,0,0,0,0);
	
  DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[0],0,0,0,0,0);
	DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[1],0,0,0,0,0);
	DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[2],0,0,0,0,0);
	DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[3],0,0,0,0,0);



}


void Motor_ON(){

	
	GIM_Motor_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&GIM_8108_Motor[0],0,0,0,0,Control_Info.L_Leg_Info.Moment.T);
  GIM_Motor_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&GIM_8108_Motor[1],0,0,0,0,Control_Info.R_Leg_Info.Moment.T);

//    DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[0],0,0,0,0,Control_Info.L_Leg_Info.SendValue.T1);		
//    DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[1],0,0,0,0,Control_Info.L_Leg_Info.SendValue.T2);
//    DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[2],0,0,0,0,Control_Info.R_Leg_Info.SendValue.T2);
//		DM_Motor_CAN_TxMessage(&FDCAN2_TxFrame,&DM_8009_Motor[3],0,0,0,0,Control_Info.R_Leg_Info.SendValue.T1);
}